#include <stdlib.h>
#include "BSpline.h"
#include <FL/fl_ask.H>
#include <math.h>
#include <FL/gl.h>
#include <GL/glu.h>

//------------------------------------------------------------	GetPoint()
// To handle the clamped curves i imagine the curve to have 3
// extra control points at the beginning of the curve, and 3 extra
// at the end. To simply the iteration of the points, i start
// the indices at -3. Obviously if i use that value as an array
// index it will crash horribly. Instead i use a function that
// returns either the first, or last point if the index requested
// is out of range. This basically simplifies our curve calculation
// greatly!!
//

   /* // draw curve hull
    glPointSize(3);
    glColor3f(0.4,0.1,0.1);

    glBegin(GL_LINE_STRIP);
    for(int i=0;i!=NUM_POINTS;++i)
    {
        glPointSize(10);
        glVertex3fv( spline.points[i] );
    }
    glEnd();

    glColor3f(1,1,1);

    // begin drawing our curve
    //glBegin(GL_LINE_STRIP);
    glColor3f(0,1,1);
    spline.draw();


    glBegin(GL_POINTS);
    glColor3f(1,0,0);
    //for(int start_cv=-3,j=0;j!=5;++j,++start_cv)
        for(float x=0.2f;x<=16.9f;x+=0.2)
        {
            glVertex3fv( spline.interpolate(x, 0) );
            //spline.interpolate(x);
        }
    glEnd();
    //glVertex3fv( spline.points[NUM_POINTS-1] );
    glEnd();

	// draw CV's
	glBegin(GL_POINTS);
	for(int i=0;i!=NUM_POINTS;++i) {
		glVertex3fv( spline.points[i] );
	}
	glEnd();


*/



BSpline::BSpline()
{
    points[0][0] = 2;
    points[0][1] = 2;
    points[0][3] = 00;

    points[1][0] = 5;
    points[1][1] = 15;
    points[1][3] = 0;

    points[2][0] = 5.01;
    points[2][1] = 0;
    points[2][3] = 00;

    points[3][0] = 14.35;
    points[3][1] = 9;
    points[3][3] = 0;

    points[4][0] = 15;
    points[4][1] = 5;
    points[4][3] = 0;

		/*{ 10,10,0 },
		{  5,10,2 },
		{ -5,0,0 },
		{-10,5,-2}*/


}


int BSpline::findClosestPoint( float x )
{
    float closestDistance = 100000000;
    int bestI = 0;


    for (int i=0; i<NUM_POINTS; i++)
    {
        float d = x - points[i][0];
        //fl_alert("%d =- %f",i, closestDistance);
        if ( d >= 0 && d<closestDistance)
        {
            //fl_alert("%d =- %f",i, d);
            closestDistance = d;
            bestI = i;
            //break;
        }
    }
//fl_alert("best %d =- %f",bestI, closestDistance);
    return bestI;
}

float BSpline::getMaxX()
{
    return 0;
}

float BSpline::getLength()
{
    return points[NUM_POINTS-1][0] - points[0][0];
}

float* BSpline::getPhantomStartPoint()
{
    float * returnPoint = new float[3];

    float xDiff = points[1][0] - points[0][0];
    float yDiff = points[1][1] - points[0][1];
    float zDiff = points[1][2] - points[0][2];

    returnPoint[0] = points[0][0] - xDiff;
    returnPoint[1] = points[0][1] - yDiff;
    returnPoint[2] = points[0][2] - zDiff;

    return returnPoint;
}

float* BSpline::getPhantomEndPoint()
{
    float * returnPoint = new float[3];

    float xDiff = points[NUM_POINTS-1][0] - points[NUM_POINTS-2][0];
    float yDiff = points[NUM_POINTS-1][1] - points[NUM_POINTS-2][1];
    float zDiff = points[NUM_POINTS-1][2] - points[NUM_POINTS-2][2];

    returnPoint[0] = points[NUM_POINTS-1][0] + xDiff;
    returnPoint[1] = points[NUM_POINTS-1][1] + yDiff;
    returnPoint[2] = points[NUM_POINTS-1][2] + zDiff;

    return returnPoint;
}

 float* BSpline::getPoint(int i)
{
	// return 1st point
	if (i<0) {
	    return points[0];
		//return	getPhantomStartPoint();
	}
	// return last point
	if (i<NUM_POINTS)
		return points[i];

    return points[NUM_POINTS-1];
	//return getPhantomEndPoint();
}

#define NUM_SEGMENTS (NUM_POINTS+1)

void BSpline::draw()
{
    glBegin(GL_LINE_STRIP);
    float * returnPoint = new float[3];
    // the level of detail for the curve
    unsigned int LOD=20;

    	for(int start_cv=-3,j=0;j!=NUM_SEGMENTS;++j,++start_cv)
	{
	    //start_cv=-2;
		// for each section of curve, draw LOD number of divisions
		for(int i=0;i<=LOD;++i) {

			// use the parametric time value 0 to 1 for this curve
			// segment.
			float t = (float)i/LOD;

			// the t value inverted
			float it = 1.0f-t;

			// calculate blending functions for cubic bspline
			float b0 = it*it*it/6.0f;
			float b1 = (3*t*t*t - 6*t*t +4)/6.0f;
			float b2 = (-3*t*t*t +3*t*t + 3*t + 1)/6.0f;
			float b3 =  t*t*t/6.0f;

			// calculate the x,y and z of the curve point
			returnPoint[0] = b0 * getPoint( start_cv + 0 )[0] +
					  b1 * getPoint( start_cv + 1 )[0] +
					  b2 * getPoint( start_cv + 2 )[0] +
					  b3 * getPoint( start_cv + 3 )[0] ;

			returnPoint[1] = b0 * getPoint( start_cv + 0 )[1] +
					  b1 * getPoint( start_cv + 1 )[1] +
					  b2 * getPoint( start_cv + 2 )[1] +
					  b3 * getPoint( start_cv + 3 )[1] ;

			returnPoint[2] = b0 * getPoint( start_cv + 0 )[2] +
					  b1 * getPoint( start_cv + 1 )[2] +
					  b2 * getPoint( start_cv + 2 )[2] +
					  b3 * getPoint( start_cv + 3 )[2] ;

			// specify the point
			glVertex3fv( returnPoint );
		}
	}
	glVertex3fv( points[NUM_POINTS-1] );
	glEnd();

}


float BSpline::getXForStartCV(int start_cv)
{
    float b0 = 1.0f/6.0f;
    float b1 = 4/6.0f;
    float b2 = 1/6.0f;

    // calculate the x of the curve point
    return b0 * getPoint( start_cv + 0 )[0] +
                     b1 * getPoint( start_cv + 1 )[0] +
                     b2 * getPoint( start_cv + 2 )[0];// +
             // b3 * getPoint( start_cv + 3 )[0] ;
}

float BSpline::getTFromX(float x)
{


   // return      getXForStartCV( getStartCVFromX(x) );

  //  /(getXForStartCV( getStartCVFromX(x)+1 )  -  getXForStartCV( getStartCVFromX(x) ));

/*
    float PrevXStart_cv, NextXStart_cv;
    //find out what segment
    int start_cv;
    for (start_cv=-2; start_cv<=(NUM_SEGMENTS-3); start_cv++)
    {
        NextXStart_cv = getXForStartCV(start_cv);
        if (NextXStart_cv > x)
            break;
    }

    //if (start_cv = -3) start_cv = -2;

    PrevXStart_cv = getXForStartCV(start_cv-1);

    float segmentXGap = NextXStart_cv - PrevXStart_cv;

    return (x - PrevXStart_cv) / segmentXGap;
*/
    //return start_cv-1;
    //start_cv now is the right segment
}

int BSpline::getStartCVFromX(float x)
{
    //find out what segment
    int start_cv;
    for (start_cv=-2; start_cv<=(NUM_SEGMENTS-3); start_cv++)
    {
        //NextXStart_cv = getXForStartCV(start_cv);
        if (getXForStartCV(start_cv) > x)
            break;
    }


    return start_cv-1;
    //start_cv now is the right segment
}

float* BSpline::interpolate(float x, int s)
{

    float * returnPoint = new float[3];
    if (x >= points[NUM_POINTS-1][0])
    {
        //fl_alert("%f", points[NUM_POINTS-1][0]);
        return points[NUM_POINTS-1];
    }




    //x *= NUM_SEGMENTS;
    int start_cv = getStartCVFromX(x);//floor(x)-2;// findClosestPoint(x) -2 ;


    returnPoint[0] = getXForStartCV(start_cv);
    returnPoint[1] = 1;
    returnPoint[2] = 0;
    return returnPoint;
   /* if (start_cv != -2 )
    {
        returnPoint[1] = 0;

        return returnPoint;
    }*/
     //fl_alert("%f", getTFromX(2.791));
  // float t = (x-floor(x)) ;//(float)(x - getPoint(start_cv)[0])/  ( getPoint(start_cv+3)[0] - getPoint(start_cv)[0] );
    float t = getTFromX(x);
    //fl_alert("start_cv = %d, a %f, b  %f, c  %f, d  %f", start_cv, getPoint( start_cv + 0 )[0], getPoint( start_cv + 1 )[0], getPoint( start_cv + 2 )[0], getPoint( start_cv + 3 )[0]);
    //float t = (float)x/5.0;

    // the t value inverted
    float it = 1.0f-t;

    // calculate blending functions for cubic bspline
    float b0 = it*it*it/6.0f;
    float b1 = (3*t*t*t - 6*t*t +4)/6.0f;
    float b2 = (-3*t*t*t +3*t*t + 3*t + 1)/6.0f;
    float b3 =  t*t*t/6.0f;


    // calculate the x,y and z of the curve point
    returnPoint[0] = b0 * getPoint( start_cv + 0 )[0] +
              b1 * getPoint( start_cv + 1 )[0] +
              b2 * getPoint( start_cv + 2 )[0] +
              b3 * getPoint( start_cv + 3 )[0] ;

    returnPoint[1] = b0 * getPoint( start_cv + 0 )[1] +
              b1 * getPoint( start_cv + 1 )[1] +
              b2 * getPoint( start_cv + 2 )[1] +
              b3 * getPoint( start_cv + 3 )[1] ;

    returnPoint[2] = b0 * getPoint( start_cv + 0 )[2] +
              b1 * getPoint( start_cv + 1 )[2] +
              b2 * getPoint( start_cv + 2 )[2] +
              b3 * getPoint( start_cv + 3 )[2] ;

   // fl_alert("y = %f", returnPoint[1]);

    return returnPoint;

}

